
#include "CANDriver.h"
#include <inc/lm3s9d90.h>
#include <inc/hw_ints.h>
#include "inc/hw_can.h"
#include <inc/hw_memmap.h>
#include <inc/hw_types.h>
#include <driverlib/debug.h>
#include <driverlib/gpio.h>
#include <driverlib/pin_map.h>
#include <driverlib/rom.h>
#include <driverlib/sysctl.h>
#include <driverlib/interrupt.h>
#include <driverlib/can.h>
#include <string.h>
#include "../Datafile.h"

extern SDataFile datafile;

void CANDriver::Init()
{
	// set pins
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
	GPIOPinConfigure(GPIO_PF0_CAN1RX);
	GPIOPinConfigure(GPIO_PF1_CAN1TX);
	GPIOPinTypeCAN(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	// enable CAN1
	SysCtlPeripheralEnable(SYSCTL_PERIPH_CAN1);
	CANInit(CAN1_BASE);
	CANBitRateSet(CAN1_BASE, SysCtlClockGet(), 1000000);

	// enable interrupts?
	CANIntEnable(CAN1_BASE, CAN_INT_MASTER | CAN_INT_ERROR | CAN_INT_STATUS);
	IntEnable(INT_CAN1);

	// Enable the CAN for operation.
	CANEnable(CAN1_BASE);

	// Add RX FIFO
	tCANMsgObject sCANMessage;
	sCANMessage.ulMsgID = 0;   // receive ALL
	sCANMessage.ulMsgIDMask = 0;
	sCANMessage.ulFlags = MSG_OBJ_EXTENDED_ID | MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER | MSG_OBJ_FIFO; // generate RX INT
	sCANMessage.ulMsgLen = 8;       // allow up to 8 bytes
	CANMessageSet(CAN1_BASE, 1, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 2, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 3, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 4, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 5, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 6, &sCANMessage, MSG_OBJ_TYPE_RX);
	CANMessageSet(CAN1_BASE, 7, &sCANMessage, MSG_OBJ_TYPE_RX);
	sCANMessage.ulFlags = MSG_OBJ_EXTENDED_ID | MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // last message in FIFO
	CANMessageSet(CAN1_BASE, 8, &sCANMessage, MSG_OBJ_TYPE_RX);
}

void CANDriver::SendMessage(int objectIdx, int msgID, float data)
{
	unsigned char buff[8];
	memcpy((void*)(buff), &data, sizeof(data));
	SendMessage(objectIdx, msgID, buff, 8);
}

void CANDriver::SendMessage(int objectIdx, int msgID, unsigned char data[8], int len)
{
	tCANMsgObject sCANMessage;

	sCANMessage.ulMsgID = msgID;                        // CAN message ID
	sCANMessage.ulMsgIDMask = 0;                    // no mask needed for TX
	sCANMessage.ulFlags = MSG_OBJ_EXTENDED_ID;// | MSG_OBJ_TX_INT_ENABLE;    // enable interrupt on TX
	sCANMessage.ulMsgLen = len;
	sCANMessage.pucMsgData = data;             // ptr to message content

	CANMessageSet(CAN1_BASE, objectIdx, &sCANMessage, MSG_OBJ_TYPE_TX);
}

void CANDriver::SetRXMessage(int objectIdx, int msgID, int len)
{
	tCANMsgObject sCANMessage;

	sCANMessage.ulMsgID = msgID;                        // CAN message ID
	sCANMessage.ulMsgIDMask = 0;
	sCANMessage.ulFlags = MSG_OBJ_EXTENDED_ID;
	sCANMessage.ulMsgLen = len;       // size of message is 4

	CANMessageSet(CAN1_BASE, objectIdx, &sCANMessage, MSG_OBJ_TYPE_RX);
}

bool CANDriver::ReadMessage(int objectIdx, float* data)
{
	unsigned char buff[8];

	bool newData = ReadMessage(objectIdx, buff);
	memcpy(data, &buff, sizeof(*data));

	return newData;
}

bool CANDriver::ReadMessage(int objectIdx, double* data)
{
	unsigned char buff[8];

	bool newData = ReadMessage(objectIdx, buff);
	memcpy(data, &buff, sizeof(*data));

	return newData;
}

bool CANDriver::ReadMessage(int objectIdx, unsigned char data[8])
{
	tCANMsgObject sCANMessage;
	sCANMessage.pucMsgData = data;

	CANMessageGet(CAN1_BASE, objectIdx, &sCANMessage, 1 );
	bool newData = sCANMessage.ulFlags & MSG_OBJ_NEW_DATA;

	return newData;
}

int ctr1 = 0;
int ctr2 = 0;
int ctr3 = 0;
int ctr4 = 0;
int ctr5 = 0;
int ctr6 = 0;
int ctr7 = 0;
int ctr8 = 0;
extern "C" void CANDriverINTHandler(void)
{
	// Read the CAN interrupt status to find the cause of the interrupt
	unsigned long ulStatus = CANIntStatus(CAN1_BASE, CAN_INT_STS_CAUSE);

	// If the cause is a controller status interrupt, then get the status
	if(ulStatus == CAN_INT_INTID_STATUS)
	{
		// Read the controller status.  This will return a field of status
		// error bits that can indicate various errors.
		ulStatus = CANStatusGet(CAN1_BASE, CAN_STS_CONTROL);
		// TODO: handle error!
	}
	else if(ulStatus >= 1 && ulStatus <= 8) // Check if the cause is from RX FIFO objects (1...8)
	{
		// check all objects
		for(int i=1; i!=8; i++)
		{
			unsigned char data[8];
			tCANMsgObject sCANMessage;
			sCANMessage.pucMsgData = data;
			CANMessageGet(CAN1_BASE, i, &sCANMessage, 1);
			if( sCANMessage.ulFlags & MSG_OBJ_NEW_DATA )
			{
				// process data from this message object
				if( i == 1) ctr1++;
				if( i == 2) ctr2++;
				if( i == 3) ctr3++;
				if( i == 4) ctr4++;
				if( i == 5) ctr5++;
				if( i == 6) ctr6++;
				if( i == 7) ctr7++;
				if( i == 8) ctr8++;

				int ID = sCANMessage.ulMsgID;
				// convert to float
				float64 dataDbl;
				float dataFlt;
				memcpy(&dataFlt, data, sizeof(float));
				memcpy(&dataDbl, data, sizeof(float64));

				// IO
				if( ID == 0x10120 ) datafile.PWMOut1 = dataFlt;
				if( ID == 0x10121 ) datafile.PWMOut2 = dataFlt;
				if( ID == 0x10122 ) datafile.PWMOut3 = dataFlt;
				if( ID == 0x10123 ) datafile.PWMOut4 = dataFlt;
				if( ID == 0x10300 ) datafile.Sonar = dataFlt;
				if( ID == 0x10301 ) datafile.IRNear = dataFlt;
				if( ID == 0x10302 ) datafile.IRFar = dataFlt;
				if( ID == 0x10310 ) datafile.Current = dataFlt;
				if( ID == 0x10311 ) datafile.Voltage = dataFlt;
				if( ID == 0x10312 ) datafile.Power = dataFlt;
				// IMU
				if( ID == 0x30300 ) datafile.gyroRollX = dataFlt;
				if( ID == 0x30301 ) datafile.gyroPitchY = dataFlt;
				if( ID == 0x30302 ) datafile.gyroYawZ = dataFlt;
				if( ID == 0x30303 ) datafile.accX = dataFlt;
				if( ID == 0x30304 ) datafile.accY = dataFlt;
				if( ID == 0x30305 ) datafile.accZ = dataFlt;
				if( ID == 0x30306 ) datafile.magX = dataFlt;
				if( ID == 0x30307 ) datafile.magY = dataFlt;
				if( ID == 0x30308 ) datafile.magZ = dataFlt;
				if( ID == 0x30310 ) datafile.Roll = dataFlt;
				if( ID == 0x30311 ) datafile.Pitch = dataFlt;
				if( ID == 0x30312 ) datafile.Yaw = dataFlt;
				if( ID == 0x30320 ) datafile.AbsPressure = dataFlt;
				if( ID == 0x30321 ) datafile.DiffPressure = dataFlt;
				if( ID == 0x30322 ) datafile.Temperature = dataFlt;
				if( ID == 0x30330 ) datafile.gpsLon = dataDbl;
				if( ID == 0x30331 ) datafile.gpsLat = dataDbl;
				if( ID == 0x30332 ) datafile.gpsAlt = dataFlt;
				if( ID == 0x30333 ) datafile.gpsSatCount = dataFlt;
				if( ID == 0x30335 ) datafile.gpsVelX = dataFlt;
				if( ID == 0x30336 ) datafile.gpsVelY = dataFlt;
				// XBee
				if( ID == 0x30350 ) datafile.XBeeLoopTimeMS = dataFlt;
				if( ID == 0x30351 ) datafile.XBeeTotalTime = dataDbl;
				if( ID == 0x30352 ) datafile.XBeeLoopTimeMSMAX = dataFlt;
				//if( ID == 0x40350 ) datafile.XBeeLoopTimeMS = dataFlt;
				//if( ID == 0x40351 ) datafile.XBeeTotalTime = dataDbl;
				//if( ID == 0x40352 ) datafile.XBeeLoopTimeMSMAX = dataFlt;
				// XXX - > Add more data
				if( ID == 0x30500) datafile.ExtraData[0] = dataFlt;
				if( ID == 0x30501) datafile.ExtraData[1] = dataFlt;
				if( ID == 0x30502) datafile.ExtraData[2] = dataFlt;
				if( ID == 0x30503) datafile.ExtraData[3] = dataFlt;
				if( ID == 0x30504) datafile.ExtraData[4] = dataFlt;
				if( ID == 0x30505) datafile.ExtraData[5] = dataFlt;
				if( ID == 0x30506) datafile.ExtraData[6] = dataFlt;
				if( ID == 0x30507) datafile.ExtraData[7] = dataFlt;
				if( ID == 0x30508) datafile.ExtraData[8] = dataFlt;
				if( ID == 0x30509) datafile.ExtraData[9] = dataFlt;
			}
		}
	}
	else
	{
		// Spurious interrupt handling can go here.
	}
}
